#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "robo/Encoder.h"
#include "robo/Motor.h"
#include <string.h>

using namespace robo;
ros::Subscriber	enc_sub;
ros::Publisher	mot_pub;
ros::Publisher	int_pub;

//Callback function for the "/encoder" topic. Prints the enconder value and randomly changes the motorspeed.
void recive_encoder(const Encoder::ConstPtr &msg)
{
	printf("got encoder L:%i , R:%i\n",msg->left,msg->right);
	Motor motor;
	motor.right	= (0.5f-float(rand()%1000)/1000.0f)*2.0f;//random motorspeed[-1,1]
	motor.left	= (0.5f-float(rand()%1000)/1000.0f)*2.0f;//random motorspeed[-1,1]
	mot_pub.publish(motor);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "FakeMotorsTest");//Creates a node named "FakeMotorsTest"
	ros::NodeHandle n;
	enc_sub = n.subscribe("/serial/encoder", 1000, recive_encoder);//when "/encoder" topic is revieved call recive_encoder function
	mot_pub = n.advertise<Motor>("/serial/motor_pwm", 100000);//used to publish a topic that changes the motorspeed
	int_pub = n.advertise<std_msgs::Int32>("/serial/encoder/interval", 100000);//used to publish a topic that changes the intervall between the "/encoder" topics published.

	ros::Rate loop_rate(10000);
	//The loop randomly changes the intervall with wich the "/encoder" topic is published.
	
	struct timeval start, end;
	while(ros::ok()){
		gettimeofday(&start, NULL);
		std_msgs::Int32 interval;
		interval.data = 10+rand()%991;//very random intervall [10,1000] ms
		int_pub.publish(interval);
		gettimeofday(&end, NULL);
		while((end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < 5000000){//Sleep 5 seconds
			loop_rate.sleep();
			ros::spinOnce();
			gettimeofday(&end, NULL);
		}
	}
	return 0;
}
